#include "is_homography_K.h"
#include<cmath>

IsHomographyK::IsHomographyK(void)
{
    /*
    cout<<"\nDEBUG, class H: Entering default constructor.\n";
    kalmanType myS = 1;
    kalmanType myTheta = 0;
    kalmanType myT[2] = {0, 0};
    kalmanType mySThetaTArray[4] = {myS, myTheta, myT[0], myT[1]};
    */
}

IsHomographyK::~IsHomographyK(void)
{

}
/*
 * ABOUT : This function computes the Homography Matrix.
 * 
 */
void IsHomographyK::getHomographyMat(kalmanType* pLpfQuaternion, kalmanType* pKcQuaternion, kalmanType* pArrayCamInternalMatrix)
{
#if IS_DEBUG
    prints("\nDebug: Entering getHomographyMat function...\n");
#endif
	//kalmanType* pIsHomographyKMat = IS_HomographyMat;
    // creating intermediate variables.
    kalmanType lpfQ[QUAT_LENGTH];       // low passed quaternion.
    kalmanType kcQ[QUAT_LENGTH];        // kalman corrected quaternion.
    kalmanType curK[K_MATRIX_LENGTH];   // current camera matrix.
    kalmanType prevK[K_MATRIX_LENGTH];  // previous camera matrix.
    
    kalmanType* pK;

    for(int i = 0; i < 4; i++)    {
        lpfQ[i] = *pLpfQuaternion;
        pLpfQuaternion++;
        kcQ[i] = *pKcQuaternion;
        pKcQuaternion++;
    }
   
#if IS_DEBUG
    // DEBUG : printing unnormalised quaternions.
    prints("\n\r--DEBUG: Printing unnormalised quaternions.\n");
    for(int i = 0; i < 4; i++)
    {
        prints("lpfQ[%d] : %lf", i, lpfQ[i]);
    }
    for(int i = 0; i < 4; i++)
    {
        prints("kcQ[%d] : %lf" , i, kcQ[i]);
    }
#endif
    IsQuaternionK::normalizeQuatK(lpfQ);
    IsQuaternionK::normalizeQuatK(kcQ);
    
    // Step 1 : Constust PrevCamera and CurrentCamera Matrix from array
    pK = pArrayCamInternalMatrix; // previous camera
    for(int i = 0; i < 9; i++)
    {
        prevK[i] = *pK;
        pK++;
    }
    pK = pArrayCamInternalMatrix;
    pK = pK + 9;
    for(int i = 0; i < 9; i++)
    {
        curK[i] = *pK;
        pK++;
    }
#if IS_DEBUG
    prints("\n--DEBUG: Printing camera matrix.\n");
    for(int i = 0; i < 9; i++)
    {
        prints("curK[%d] : %lf", i, curK[i]);
    }
    prints("\n\r");
    for(int i = 0; i < 9; i++)
    {
        prints("prevK[%d] : %lf", i, prevK[i]);
    }

#endif
    /* 
     * Now we calculate the rotation Matrix between prevQ and curQ
    */

   /*
    * calculate the inverse of kcQ.
    */
    IsQuaternionK::invertQuaternion(kcQ);

    // calculate hamiltion product.
    kalmanType myHamiltonProduct[4];
    kalmanType* pHamiltonProduct = myHamiltonProduct;
    pHamiltonProduct = IsQuaternionK::hamiltonionProduct(lpfQ, kcQ);

    for(int i =0; i < 4; i++)
    {
        myHamiltonProduct[i] = *pHamiltonProduct;
        pHamiltonProduct++;
    }

    // calculate rotation matrix from it.
    kalmanType myRotationMat[9];
    //kalmanType* pRotationMat;
    IsQuaternionK::quatToRotationMat(myHamiltonProduct);
/*
	for(int i =0; i < 9; i++)
    {
        myRotationMat[i] = *pRotationMat;
        pRotationMat++;
    }     
*/
#if IS_DEBUG

    prints("\n\r--DEBUG: Printing rotation Matrix.\n");
    for(int i=0; i<9; i++)
    {
        prints("myRotationMat[%d]: %lf", i , myRotationMat[i]);
    }
    
#endif

    //Calculate the inverse of rotationMatrix
    kalmanType invRotMat[R_MATRIX_LENGTH];
    //kalmanType* pInvRotMat = myRotationMat;
    IsMatrixK::calcInverseMat(myRotationMat, R_MATRIX_LENGTH);
    // construct this rotation matrix.
    for(int i = 0; i < R_MATRIX_LENGTH; i++)
    {
        invRotMat[i] = IsMatrixK::IS_InverseMatrix[i];
    }
#if 0
    cout<<"\nDEBUG : - rotation and its inverse mat.\n";
    for(int i = 0; i < R_MATRIX_LENGTH; i++)
    {
        cout<<myRotationMat[i]<<"\t";
    }
    cout<<"\n\n";
    for(int i = 0; i < R_MATRIX_LENGTH; i++)
    {
        cout<<invRotMat[i]<<"\t";
    }
#endif
      
   /* This block will calculate the homography matrix. */
    // Inverse prevK.
    kalmanType invPrevK[R_MATRIX_LENGTH];
    //kalmanType* pInvPrevK = IsMatrixK::IS_InverseMatrix;
    IsMatrixK::calcInverseMat(prevK, K_MATRIX_LENGTH);

    // copy this into local matrix.
    for(int i = 0 ; i < R_MATRIX_LENGTH; i++)
    {
        invPrevK[i] = IsMatrixK::IS_InverseMatrix[i];
    }
#if 0
    cout<<"\n--DEBUG : Printing camera matrices.\n";
    for(int i = 0 ; i < R_MATRIX_LENGTH; i++)
    {
        cout<<curK[i]<<"\t";
    }
    cout<<endl;
    for(int i = 0 ; i < R_MATRIX_LENGTH; i++)
    {
        cout<<prevK[i]<<"\t";
    }
    cout<<"--DEBUG : Printing inverse matrix of curK."<<endl;
    for(int i = 0 ; i < R_MATRIX_LENGTH; i++)
    {
        cout<<invCurK[i]<<"\t";
    }
#endif
    // Final calculation.
	IsMatrixK::multMatrix(invRotMat, invPrevK, 3);
    IsMatrixK::multMatrix(curK, IS_MultMatrix, 3);
    // copy it into local matrix.
    for(int i = 0; i < H_MATRIX_LENGTH; i++)
    {
        IsHomographyK::IS_HomographyMat[i] = IS_MultMatrix[i];
    }

#if 0
    cout<<"\n--DEBUG : Printing Homography matrices.\n";
    for(int i = 0 ; i < H_MATRIX_LENGTH; i++)
    {
        cout<<IsHomographyK::IS_HonographyMat[i]<<"\t";
    }
    cout<<endl;
#endif
    //pIsHomographyKMat = IsHomographyK::IS_HomographyMat;
  
}

/******************************************************************************
 * Decompose the homography matrix.
 * ABOUT : Into S, theta and T.
 ******************************************************************************/
void IsHomographyK::decomposeHomographyMat(double *pHomographyMat)
{
    //cout<<"\nEntering decomposeHomographyMatrix...\n";
    // construct a local array to hold these values.
    kalmanType H[H_MATRIX_LENGTH];
    for(int i = 0; i < H_MATRIX_LENGTH; i++)
    {
        H[i] = *pHomographyMat;
        IsHomographyK::IS_HomographyMat[i] = *pHomographyMat;
        pHomographyMat++;
    }

    // Local variable to be used in calculations.
    kalmanType h33 = H[8];
    kalmanType v[2] = {H[6], H[7]};
    kalmanType t[2] = {H[2]/h33, H[5]/h33};
    kalmanType tvMat[4] = {t[0]*v[0], t[0]*v[1], t[1]*v[0], t[1]*v[1]}; // OK.

    kalmanType matB[4] = {H[0], H[1], H[3], H[4]};
    for(int i = 0; i < 4; i++)
    {
        matB[i] =  matB[i] - tvMat[i];  // Tested O.K. with Matlab.
    }

    /*
     * We now solve these equations.
     * AX = B
     * where A = [cos x,    0,      0
                  0    ,  cos x, -sin x
                  0    , sin x, cos x]
     * and B = [ 
                matA[0]
                matA[1]
                matA[2]
               ]

     * x = atan(matA[2]/matA[0];
     */
    kalmanType x = atan(matB[2]/matB[0]); // theta = tan^-1(c/a)              
    /*
    cout<<"\nPrinting Theta : "<<x;
    */
    kalmanType A[K_MATRIX_LENGTH] = { 
                                      cos(x), 0, 0,
                                      0, cos(x), -sin(x),
                                      0, sin(x), cos(x)
                                    };
    kalmanType B[3] = {
                        matB[0],
                        matB[1],
                        matB[3]
                      };
    kalmanType invA[K_MATRIX_LENGTH];
    kalmanType X[3] = {0 , 0, 0}; // array to hold variables.
    //kalmanType* pInvA = invA;
    //kalmanType* pX = X;

    //pInvA = IsMatrixK::calcInverseMat(A, K_MATRIX_LENGTH);
    for(int i = 0; i < K_MATRIX_LENGTH; i++)
    {
        invA[i] = IsMatrixK::IS_InverseMatrix[i]; // Tested ok with MATLAB.
    }
    // Caculate X = inv(A)*B
    for(int i = 0; i < 3; i++)              // Tested O.K. with Matlab.
    {
        for(int j = 0; j < 3; j++)
        {
            kalmanType temp = invA[j+3*i]*B[j];
            X[i] = X[i] + temp;
        }
    }

    /* Caculate s, theta and T. */
    IsHomographyK::IS_S = sqrt(X[0]*X[2]);
    IsHomographyK::IS_Theta = x;
    IsHomographyK::IS_T[0] =t[0]; IsHomographyK::IS_T[1] =t[1];
    //cout<<"\n------"<<IS_S<<"--"<<X[0];
    
#if 0
    
    cout<<"\nFOR DEBUGGING ..\n\n";
    cout<<"\nDEBUG: Decompose homography...\n";
    cout<<"\n\n";
    for(int i = 0; i < 4; i++)
    {
        cout<<matB[i]<<"\t";
        //cout<<tvMat[i]<<"\t"; // Tested OK with MATLAB
    }
    
    cout<<"\n\n Mat and invMat"<<endl;
    /*
    for(int i = 0; i < K_MATRIX_LENGTH; i++)
    {
        cout<<H[i]<<"\t";
    }
    */
    cout<<"\n\n";
    for(int i = 0; i < K_MATRIX_LENGTH; i++)
    {
        cout<<A[i]<<"\t";
     }
    cout<<"\n\n";
    for(int i = 0; i < K_MATRIX_LENGTH; i++)
    {
        cout<<invA[i]<<"\t";
    }
    cout<<"\n\n Variables";
    for(int i = 0; i < 3; i++)
    {
        cout<<X[i]<<"\t";
    }
    cout<<"\n\nEnd ...";

#endif
    IsHomographyK::IS_SThetaTArray[0] = IsHomographyK::IS_S;
    IsHomographyK::IS_SThetaTArray[1] = IsHomographyK::IS_Theta;
    IsHomographyK::IS_SThetaTArray[2] = IsHomographyK::IS_T[0];
    IsHomographyK::IS_SThetaTArray[3] = IsHomographyK::IS_T[1];  
    
}
